/* Copyright 2024 INRIA.
 * Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 * Siconos is a free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * Siconos is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with Siconos; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * Contact: Vincent ACARY, siconos-team@lists.gforge.inria.fr
 */
#define _POSIX_C_SOURCE 200112L

#include "CSparseMatrix.h"

#include <assert.h>  // for assert
#include <float.h>   // for DBL_EPSILON
#include <math.h>    // for fabs
#include <stdio.h>   // for fprintf, sscanf, printf, NULL, fgets
#include <stdlib.h>  // for realloc, exit, free, malloc, EXIT_FAILURE
#include <string.h>  // for strtok_r, memcpy, strncmp

#include "ldl.h"
#include "numerics_verbose.h"  // for CHECK_IO
#if defined(_WIN32) || defined(_WIN64)
/* We are on Windows */
#define strtok_r strtok_s
#endif

#if defined(__cplusplus)
#undef restrict
#include <sys/cdefs.h>  // for __restrict
#define restrict __restrict
#endif
/* #define DEBUG_NOCOLOR */
/* #define DEBUG_STDOUT */
/* #define DEBUG_MESSAGES */
#include "siconos_debug.h"  // for DEBUG_PRINTF

#ifdef DEBUG_MESSAGES
#include "NumericsVector.h"
#endif

double CSparseMatrix_get_value(const CSparseMatrix *A, CS_INT i, CS_INT j) {
  CS_INT *Ai = A->i;
  CS_INT *Ap = A->p;
  double *Ax = A->x;

  for (CS_INT row = Ap[j]; row < Ap[j + 1]; row++) {
    if (i == Ai[row]) return Ax[row];
  }
  return 0.0;
}

void CSparseMatrix_write_in_file_python(const CSparseMatrix *const m, FILE *file) {
  fprintf(file, "m = %" PRCS_INT "; \n", m->m);
  fprintf(file, "n = %" PRCS_INT "; \n", m->n);
  fprintf(file, "data= [");
  for (int i = 0; i < m->m; i++) {
    fprintf(file, "[");
    for (int j = 0; j < m->n; j++) {
      fprintf(file, "%32.24e,\t ", CSparseMatrix_get_value((CSparseMatrix *)m, i, j));
    }
    fprintf(file, "],\n");
  }
  fprintf(file, "]");
}

/* y = alpha*A*x+beta*y */
int CSparseMatrix_aaxpby(const double alpha, const CSparseMatrix *A, const double *restrict x,
                         const double beta, double *restrict y) {
  CS_INT n, m, *Ap, *Ai;
  double *Ax;
  if (!CS_CSC(A) || !x || !y) return (0); /* check inputs */
  {
    n = A->n;
    m = A->m;
    Ap = A->p;
    Ai = A->i;
    Ax = A->x;

    for (int j = 0; j < m; j++) {
      y[j] *= beta;
    }

    for (int j = 0; j < n; j++) {
      for (CS_INT p = Ap[j]; p < Ap[j + 1]; p++) {
        y[Ai[p]] += alpha * Ax[p] * x[j];
      }
    }
  }
  return 1;
}
/* A <-- alpha*A */
int CSparseMatrix_scal(const double alpha, const CSparseMatrix *A) {
  CS_INT n, *Ap;
  double *Ax;
  if (!CS_CSC(A)) return (0); /* check inputs */
  {
    n = A->n;
    Ap = A->p;
    Ax = A->x;
    for (int j = 0; j < n; j++) {
      for (CS_INT p = Ap[j]; p < Ap[j + 1]; p++) {
        Ax[p] = alpha * Ax[p];
      }
    }
  }
  return 1;
}

int CSparseMatrix_check_triplet(CSparseMatrix *T) {
  if (T->nz < 0) {
    fprintf(stderr,
            "CSparseMatrix_check_triplet :: given CSparseMatrix is not in a triplet form: nz "
            "= " CS_ID,
            T->nz);
    return 1;
  }
  CS_INT nb_row = T->m;
  CS_INT nb_col = T->n;
  CS_INT *Ti = T->i;
  CS_INT *Tp = T->p;
  int info = 0;
  CS_INT cc = 0;
  CS_INT max_row = -1;
  CS_INT max_col = -1;

  for (CS_INT indx = 0; indx < T->nz; ++indx) {
    if (Ti[indx] >= nb_row) {
      printf("CSparseMatrix_check_triplet :: matrix element " CS_ID " has a row number " CS_ID
             "  > " CS_ID " the number of rows\n",
             indx, Ti[indx], nb_row);
      info = 1;
    }
    if (Tp[indx] >= nb_col) {
      printf("CSparseMatrix_check_triplet :: matrix element " CS_ID " has a row number " CS_ID
             " > " CS_ID " the number of rows\n",
             indx, Tp[indx], nb_col);
      info = 1;
    }
    if (Tp[indx] < max_col) {
      printf("CSparseMatrix_check_csc :: " CS_ID " at index " CS_ID " > " CS_ID "\n", Tp[indx],
             indx, max_col);
    }
    if (Tp[indx] == cc) {
      if (Ti[indx] <= max_row) {
        printf("CSparseMatrix_check_triplet :: matrix element at column " CS_ID
               " has a row number " CS_ID "  > " CS_ID " the max on that column\n",
               cc, Ti[indx], max_row);
      } else {
        max_row = Ti[indx];
      }
    } else {
      cc = Tp[indx];
      max_row = -1;
    }
  }
  return info;
}

int CSparseMatrix_check_csc(CSparseMatrix *T) {
  if (T->nz != -1) {
    fprintf(stderr,
            "CSparseMatrix_check_csc :: given CSparseMatrix is not in a csc form: nz "
            "= %" PRCS_INT "\n",
            T->nz);
    return 1;
  }

  CS_INT nb_row = T->m;
  CS_INT nb_col = T->n;
  CS_INT *Ti = T->i;
  CS_INT *Tp = T->p;
  int info = 0;

  for (CS_INT j = 0; j < nb_col; ++j) {
    CS_INT max_indx = -1;
    if (Tp[j] > Tp[j + 1]) {
      printf("CSparseMatrix_check_csc :: %" PRCS_INT " at index %" PRCS_INT
             " smaller than %" PRCS_INT "\n",
             Tp[j + 1], j, Tp[j]);
    }
    for (CS_INT p = Tp[j]; p < Tp[j + 1]; ++p) {
      if (Ti[p] <= max_indx) {
        printf("CSparseMatrix_check_csc :: matrix element (%" PRCS_INT ",%" PRCS_INT
               ") at index %" PRCS_INT " has a row number < %" PRCS_INT " the previous max\n",
               Ti[p], j, p, max_indx);
        info = 1;
      } else if (Ti[p] >= nb_row) {
        printf("CSparseMatrix_check_csc :: matrix element (%" PRCS_INT ",%" PRCS_INT
               ") at index %" PRCS_INT " has a row number > %" PRCS_INT " the max\n",
               Ti[p], j, p, nb_row);
        info = 1;
      } else {
        max_indx = Ti[p];
      }
    }
  }
  return info;
}

CSparseMatrix *CSparseMatrix_spfree_on_stack(CSparseMatrix *A) {
  if (!A) return NULL; /* do nothing if A already NULL */
  cs_free(A->p);
  A->p = NULL;
  cs_free(A->i);
  A->i = NULL;
  cs_free(A->x);
  A->x = NULL;
  return NULL;
}

int CSparseMatrix_lu_factorization(CS_INT order, const cs *A, double tol,
                                   CSparseMatrix_factors *cs_lu_A) {
  assert(A);
  cs_lu_A->n = A->n;
  css *S = cs_sqr(order, A, 0);
  cs_lu_A->S = S;
  cs_lu_A->N = cs_lu(A, S, tol);

  return (S && cs_lu_A->N);
}
int CSparseMatrix_chol_factorization(CS_INT order, const cs *A,
                                     CSparseMatrix_factors *cs_chol_A) {
  assert(A);
  cs_chol_A->n = A->n;
  css *S = cs_schol(order, A);
  cs_chol_A->S = S;
  cs_chol_A->N = cs_chol(A, S);

  return (S && cs_chol_A->N);
}
int CSparseMatrix_ldlt_factorization(CS_INT order, const cs *A,
                                     CSparseMatrix_factors *cs_ldlt_A) {
  assert(A);

  CS_INT *Ap, *Ai, *Lp, *Li;
  CS_INT *Parent;
  CS_ENTRY *Ax, *Lx;
  css *S;
  csn *N;
  CS_INT n, lnz;
  DEBUG_EXPR(cs_print(A, 1););
  Ap = A->p;
  Ai = A->i;
  Ax = A->x;

  cs_ldlt_A->n = n = A->n;

  /* could be good to good cs_spalloc */

  cs_ldlt_A->N = N = cs_calloc(1, sizeof(csn)); /* allocate result N */
  cs_ldlt_A->N->L = cs_calloc(1, sizeof(cs));   /* allocate the cs struct */
  cs_ldlt_A->N->L->m = n;
  cs_ldlt_A->N->L->n = n;
  cs_ldlt_A->N->L->nz = -1;
  cs_ldlt_A->N->L->p = Lp = cs_malloc(n + 1, sizeof(CS_INT));

  cs_ldlt_A->S = S = cs_calloc(1, sizeof(css)); /* allocate result S */
  cs_ldlt_A->S->parent = Parent = cs_malloc(n + 1, sizeof(CS_INT));

  CS_INT *Lnz = cs_malloc(n, sizeof(CS_INT));
  CS_INT *Flag = cs_malloc(n, sizeof(CS_INT));

  /* ordering with amd */
  CS_INT *Perm, *PermInv;
  cs_ldlt_A->N->pinv = Perm = cs_amd(order, A); /* We used pinv to store Perm !! */
  PermInv = cs_malloc(n, sizeof(CS_INT));

  DEBUG_EXPR(for (int k = 0; k < n + 1; k++) { printf("%li\t", Perm[k]); } printf("\n"););

  /* symbolic factorization to get Lp, Parent, Lnz, and Pinv */
  LDL_symbolic(n, Ap, Ai, Lp, Parent, Lnz, Flag, Perm, PermInv);
  DEBUG_EXPR(for (int k = 0; k < n + 1; k++) { printf("%li\t", Lp[k]); } printf("\n"););
  DEBUG_EXPR(for (int k = 0; k < n; k++) { printf("%li\t", Flag[k]); } printf("\n"););

  /* factorization */
  lnz = Lp[n];
  DEBUG_PRINTF("Lp[n] = %ld\n", Lp[n]);
  cs_ldlt_A->N->L->i = Li = cs_malloc(lnz, sizeof(CS_INT));
  cs_ldlt_A->N->L->x = Lx = cs_malloc(lnz, sizeof(CS_ENTRY));

  CS_INT *Pattern = cs_malloc(n, sizeof(CS_INT));
  CS_ENTRY *D;
  cs_ldlt_A->N->B = D =
      cs_malloc(n, sizeof(CS_ENTRY)); /* We use cs_ldlt_A->N->B  for storing D !! */
  CS_ENTRY *Y = cs_malloc(n, sizeof(CS_ENTRY));
  LDL_numeric(n, Ap, Ai, Ax, Lp, Parent, Lnz, Li, Lx, D, Y, Flag, Pattern, Perm, PermInv);

  DEBUG_EXPR(cs_print(cs_ldlt_A->N->L, 1););
  DEBUG_EXPR(NV_display(D, n));

  cs_free(Lnz);
  cs_free(Flag);
  cs_free(PermInv);
  cs_free(Pattern);
  cs_free(Y);

  return (S && cs_ldlt_A->N);
}

void CSparseMatrix_free_lu_factors(CSparseMatrix_factors *cs_lu_A) {
  assert(cs_lu_A);
  if (cs_lu_A) {
    cs_lu_A->n = -1;

    cs_sfree(cs_lu_A->S);
    cs_lu_A->S = NULL;

    cs_nfree(cs_lu_A->N);
    cs_lu_A->N = NULL;

    free(cs_lu_A);
  }
}

/* Solve Ax = b with the factorization of A stored in the cs_lu_A
 * This is extracted from cs_lusol, you need to synchronize any changes! */
CS_INT CSparseMatrix_solve(CSparseMatrix_factors *cs_lu_A, double *x, double *b) {
  assert(cs_lu_A);

  CS_INT ok;
  CS_INT n = cs_lu_A->n;
  css *S = cs_lu_A->S;
  csn *N = cs_lu_A->N;
  ok = (S && N && x);
  if (ok) {
    cs_ipvec(N->pinv, b, x, n); /* x = b(p) */
    cs_lsolve(N->L, x);         /* x = L\x */
    cs_usolve(N->U, x);         /* x = U\x */
    cs_ipvec(S->q, x, b, n);    /* b(q) = x */
  }
  return (ok);
}

/* Solve Ax = B with the factorization of A stored in the cs_lu_A
 * B is a sparse matrix (CSparseMatrix_factors)
 * This is extracted from cs_lusol, you need to synchronize any changes! */
CS_INT CSparseMatrix_spsolve(CSparseMatrix_factors *cs_lu_A, CSparseMatrix *X,
                             CSparseMatrix *B) {
  assert(cs_lu_A);
  DEBUG_BEGIN("CSparseMatrix_spsolve(...)\n");

  if (!CS_CSC(X)) return 1; /* check inputs */
  if (!CS_CSC(B)) return 1; /* check inputs */

  CS_INT ok;
  CS_INT n = cs_lu_A->n;
  csn *N = cs_lu_A->N;
  if (!N) return 1;
  css *S = cs_lu_A->S;
  if (!S) return 1;

  CS_ENTRY *x, *b, *Xx, *Bx;
  CS_INT *xi, *pinv, *q, top, k, i, p, *Bp, *Bi, *Xp, *Xi;

  x = cs_malloc(n, sizeof(CS_ENTRY));    /* get CS_ENTRY workspace */
  b = cs_malloc(n, sizeof(CS_ENTRY));    /* get CS_ENTRY workspace */
  xi = cs_malloc(2 * n, sizeof(CS_INT)); /* get CS_INT workspace */

  CS_INT xnz = 0;
  Xp = X->p;
  Xi = X->i;
  Xx = X->x;
  Xp[0] = 0;
  pinv = N->pinv;
  /* --- 1. First step X = L\B ---------------------------------------------- */
  for (k = 0; k < B->n; k++) {
    /* permutation of the rows  of B(:,k) */
    for (p = B->p[k]; p < B->p[k + 1]; p++) x[B->i[p]] = B->x[p]; /* scatter B  */
    for (p = B->p[k]; p < B->p[k + 1]; p++) {
      CS_INT i_old = B->i[p];
      B->i[p] = pinv[i_old]; /* permute row indices with N->pinv */
      B->x[p] = x[i_old];
    }
    /* call spsolve */
    top = cs_spsolve(N->L, B, k, xi, x, NULL, 1); /* x = L\B(:,col) */
    /* store the result in X */
    if (Xp[k] + n - top > X->nzmax &&
        !cs_sprealloc(X, 2 * (X->nzmax) + n - top)) /* realloc X if need */
    {
      return 1; /* (cs_done(X, w, x, 0)) ;   */ /* out of memory */
    }
    Xp = X->p;
    Xi = X->i;
    Xx = X->x;
    for (p = top; p < n; p++) {
      i = xi[p];   /* x(i) is nonzero */
      Xi[xnz] = i; /* store the result in X */
      Xx[xnz++] = x[i];
    }
    Xp[k + 1] = Xp[k] + n - top;
  }
  DEBUG_EXPR(cs_print(X, 0););

  /* --- 2. First step B = U\B ---------------------------------------------- */
  DEBUG_PRINT("2- Second step B = U\\X\n");
  CS_INT bnz = 0;
  Bp = B->p;
  Bi = B->i;
  Bx = B->x;
  Bp[0] = 0;
  q = S->q;
  for (k = 0; k < X->n; k++) {
    top = cs_spsolve(N->U, X, k, xi, x, NULL, 0); /* x = U\X(:,col) */

    /* store the result in B */
    if (Bp[k] + n - top > B->nzmax && !cs_sprealloc(B, 2 * (B->nzmax) + n - top)) {
      return 1; /* (cs_done(X, w, x, 0)) ;   */ /* out of memory */
    }
    Bp = B->p;
    Bi = B->i;
    Bx = B->x;
    for (p = top; p < n; p++) {
      i = xi[p];      /* x(i) is nonzero */
      Bi[bnz] = q[i]; /* permute the indices with S->q */
      Bx[bnz++] = x[i];
    }
    Bp[k + 1] = Bp[k] + n - top;
  }
  DEBUG_EXPR(cs_print(B, 0));
  ok = 1;
  free(x);
  free(b);
  free(xi);
  DEBUG_END("CSparseMatrix_spsolve(...)\n");
  return (ok);
}

CS_INT CSparseMatrix_chol_solve(CSparseMatrix_factors *cs_chol_A, double *x, double *b) {
  assert(cs_chol_A);

  CS_INT ok;
  CS_INT n = cs_chol_A->n;
  css *S = cs_chol_A->S;
  csn *N = cs_chol_A->N;
  ok = (S && N && x);
  if (ok) {
    cs_ipvec(S->pinv, b, x, n); /* x = P*b */
    cs_lsolve(N->L, x);         /* x = L\x */
    cs_ltsolve(N->L, x);        /* x = L'\x */
    cs_pvec(S->pinv, x, b, n);  /* b = P'*x */
  }
  return (ok);
}

/* Solve Ax = B with the factorization of A stored in the cs_chol_A
 * B is a sparse matrix (CSparseMatrix_factors)
 * This is extracted from cs_lusol, you need to synchronize any changes! */
CS_INT CSparseMatrix_chol_spsolve(CSparseMatrix_factors *cs_chol_A, CSparseMatrix *X,
                                  CSparseMatrix *B) {
  DEBUG_BEGIN("CSparseMatrix_chol_spsolve(...)\n");

  if (!CS_CSC(X)) return 1; /* check inputs */
  if (!CS_CSC(B)) return 1; /* check inputs */

  CS_INT ok;
  CS_INT n = cs_chol_A->n;
  csn *N = cs_chol_A->N;
  if (!N) return 1;
  css *S = cs_chol_A->S;
  if (!S) return 1;

  CS_ENTRY *x, *b, *Xx, *Bx;
  CS_INT *xi, *pinv, top, k, i, p, *Bp, *Bi, *Xp, *Xi;

  x = cs_malloc(n, sizeof(CS_ENTRY));    /* get CS_ENTRY workspace */
  b = cs_malloc(n, sizeof(CS_ENTRY));    /* get CS_ENTRY workspace */
  xi = cs_malloc(2 * n, sizeof(CS_INT)); /* get CS_INT workspace */

  CS_INT xnz = 0;
  Xp = X->p;
  Xi = X->i;
  Xx = X->x;
  Xp[0] = 0;
  pinv = S->pinv;

  /* for (i =0; i <n; i++) printf("pinv[%li] = %li\t", i, pinv[i]); */
  /* printf("\n"); */

  /* --- 1. First step X = L\B ---------------------------------------------- */
  for (k = 0; k < B->n; k++) {
    /* permutation of the rows  of B(:,k) */
    for (p = B->p[k]; p < B->p[k + 1]; p++) x[B->i[p]] = B->x[p]; /* scatter B  */
    for (p = B->p[k]; p < B->p[k + 1]; p++) {
      CS_INT i_old = B->i[p];
      B->i[p] = pinv[i_old]; /* permute row indices with N->pinv */
      B->x[p] = x[i_old];
    }
    /* call spsolve */
    top = cs_spsolve(N->L, B, k, xi, x, NULL, 1); /* x = L\B(:,col) */
    /* store the result in X */
    if (Xp[k] + n - top > X->nzmax &&
        !cs_sprealloc(X, 2 * (X->nzmax) + n - top)) /* realloc X if need */
    {
      return 1; /* (cs_done(X, w, x, 0)) ;   */ /* out of memory */
    }
    Xp = X->p;
    Xi = X->i;
    Xx = X->x;
    for (p = top; p < n; p++) {
      i = xi[p];   /* x(i) is nonzero */
      Xi[xnz] = i; /* store the result in X */
      Xx[xnz++] = x[i];
    }
    Xp[k + 1] = Xp[k] + n - top;
  }
  DEBUG_EXPR(cs_print(X, 0););

  /* --- 2. First step B = L'\B ---------------------------------------------- */
  DEBUG_PRINT("2- Second step B = L'\\X\n");

  CSparseMatrix *LT = cs_transpose(N->L, 1);
  CS_INT bnz = 0;
  Bp = B->p;
  Bi = B->i;
  Bx = B->x;
  Bp[0] = 0;
  for (k = 0; k < X->n; k++) {
    top = cs_spsolve(LT, X, k, xi, x, NULL, 0); /* x = U\X(:,col) */

    /* store the result in B */
    if (Bp[k] + n - top > B->nzmax && !cs_sprealloc(B, 2 * (B->nzmax) + n - top)) {
      return 1; /* (cs_done(X, w, x, 0)) ;   */ /* out of memory */
    }
    Bp = B->p;
    Bi = B->i;
    Bx = B->x;
    /* permutation with S->pinv */
    cs_pvec(pinv, x, b, n); /* b = P'*x */
    // for(p = top ; p < n ; p++) b [q ? q [p] : p] = x [p] ;  /* b(q) = x */
    for (p = top; p < n; p++) {
      i = xi[p];                                           /* x(i) is nonzero */
      i = pinv[i]; /* permute the indices with S->pinv */  // to be checked carefully
      Bi[bnz] = i;
      Bx[bnz++] = b[i];
    }
    Bp[k + 1] = Bp[k] + n - top;
  }
  DEBUG_EXPR(cs_print(B, 0));
  ok = 1;
  free(x);
  free(b);
  free(xi);
  DEBUG_END("CSparseMatrix_chol_spsolve(...)\n");
  return (ok);
}

CS_INT CSparseMatrix_ldlt_solve(CSparseMatrix_factors *cs_ldlt_A, double *x, double *b) {
  assert(cs_ldlt_A);

  CS_INT ok;

  CS_INT n = cs_ldlt_A->n;
  css *S = cs_ldlt_A->S;
  csn *N = cs_ldlt_A->N;
  ok = (S && N && x);

  cs *L;
  CS_INT *Lp, *Li;
  CS_ENTRY *Lx;

  if (ok) {
    CS_INT *P = cs_ldlt_A->N->pinv; /* We used pinv to store Perm !! */
    CS_ENTRY *D = cs_ldlt_A->N->B;  /* We use cs_ldlt_A->N->B  for storing D !! */

    L = cs_ldlt_A->N->L;
    Lp = L->p;
    Li = L->i;
    Lx = L->x;

    /* solve Ax=b */
    if (P) {
      /* the factorization is LDL' = PAP' */
      LDL_perm(n, x, b, P);          /* y = Pb */
      LDL_lsolve(n, x, Lp, Li, Lx);  /* y = L\y */
      LDL_dsolve(n, x, D);           /* y = D\y */
      LDL_ltsolve(n, x, Lp, Li, Lx); /* y = L'\y */
      LDL_permt(n, b, x, P);         /* x = P'y */
    } else {
      /* the factorization is LDL' = A */
      DEBUG_EXPR(NV_display(b, n));
      DEBUG_EXPR(NV_display(D, n));
      LDL_lsolve(n, b, Lp, Li, Lx);  /* x = L\x */
      LDL_dsolve(n, b, D);           /* x = D\x */
      LDL_ltsolve(n, b, Lp, Li, Lx); /* x = L'\x */
      DEBUG_EXPR(NV_display(b, n));
    }
  }
  return (ok);
}

CSparseMatrix *CSparseMatrix_new_from_file(FILE *file) {
  CS_INT m = 0, n = 0, nzmax = 0, nz, p, j, *Ap, *Ai;
  long long foo;
  double *Ax;
  int info = 0;
  char line[2048];

  /* CHECK_IO(fscanf(file, "%20[^\n]", line ), &info); */
  /* fscanf(file, "%2047[^\n]", line ); */

  if (fgets(line, 2047, file) != NULL) {
    DEBUG_PRINTF("line = %s\n", line);
  }
  if (fgets(line, 2047, file) != NULL) {
    DEBUG_PRINTF("line = %s\n", line);
  }
  if (fgets(line, 2047, file) != NULL) {
    DEBUG_PRINTF("line = %s\n", line);
  }

  char *str1, *str2, *token, *subtoken;
  char *saveptr1, *saveptr2;
  const char s_1[2] = " ", s_2[2] = "-";
  int k;
  int is_triplet = 0;
  for (k = 1, str1 = line;; k++, str1 = NULL) {
    token = strtok_r(str1, s_1, &saveptr1);
    if (token == NULL) break;
    DEBUG_PRINTF("%d: %s\n", k, token);
    if (strncmp(token, "triplet:", 8) == 0) {
      DEBUG_PRINT(" triplet matrix\n");
      is_triplet = 1;
    }
    if (k == 1 + is_triplet) {
      int kk = 0;
      for (str2 = token;; str2 = NULL) {
        subtoken = strtok_r(str2, s_2, &saveptr2);
        if (kk == 0) {
          if (1 == sscanf(subtoken, "%lld", &foo)) m = (CS_INT)foo;
        }
        if (kk == 2) {
          if (1 == sscanf(subtoken, "%lld", &foo) && kk) n = (CS_INT)foo;
        }
        kk = kk + 1;
        if (subtoken == NULL) break;
        DEBUG_PRINTF(" --> %s\n", subtoken);
      }
      DEBUG_PRINTF("m = %li, n = %li \n", m, n);
    }

    if (k == 3 + is_triplet) {
      if (1 == sscanf(token, "%lld", &foo)) nzmax = (CS_INT)foo;
    }
    if (k == 6 && is_triplet) {
      if (1 == sscanf(token, "%lld", &foo)) nz = (CS_INT)foo;
    }
  }

  CSparseMatrix *out = cs_spalloc(m, n, nzmax, 1, is_triplet);

  if (is_triplet) {
    out->nz = nz;
  } else {
    out->nz = -1;
  }

  Ai = out->i;
  Ap = out->p;
  Ax = out->x;

  long long int val1, val2;
  double val3;
  if (out->nz < 0) {
    for (j = 0; j < n; j++) {
      /* fprintf(file,"    col %lld : locations %lld to %lld\n",  (long long int)j,  (long long
       * int)Ap [j],  (long long int)Ap [j+1]-1); */

      if (fgets(line, 2047, file) != NULL) {
        DEBUG_PRINTF("line 1 = %s\n", line);
      }
      for (k = 1, str1 = line;; k++, str1 = NULL) {
        token = strtok_r(str1, s_1, &saveptr1);
        if (token == NULL) break;
        DEBUG_PRINTF("%d: %s\n", k, token);
        if (k == 2) {
          if (1 == sscanf(token, "%lld", &val1)) {
            DEBUG_PRINTF(" j- col = %lli\n", j - val1);
          }
          assert(j - val1 == 0);
        }
        if (k == 5) {
          if (1 == sscanf(token, "%lld", &val1)) Ap[j] = (CS_INT)val1;
        }
        if (k == 7) {
          if (1 == sscanf(token, "%lld", &val2)) Ap[j + 1] = (CS_INT)val2 + 1;
        }
      }
      DEBUG_PRINTF("    col %lld : locations %lld to %lld\n", (long long int)j,
                   (long long int)Ap[j], (long long int)Ap[j + 1] - 1);

      for (p = Ap[j]; p < Ap[j + 1]; p++) {
        if (fgets(line, 2047, file) != NULL) {
          DEBUG_PRINTF("line 2 = %s\n", line);
        }

        for (k = 1, str1 = line;; k++, str1 = NULL) {
          token = strtok_r(str1, s_1, &saveptr1);
          if (token == NULL) break;
          DEBUG_PRINTF("%d: %s\n", k, token);
          if (k == 1) {
            if (1 == sscanf(token, "%lld", &val1)) Ai[p] = (CS_INT)val1;
          }
          if (k == 3) {
            if (1 == sscanf(token, "%32le", &val3)) Ax[p] = val3;
          }
        }
      }
    }
  } else {
    /* fprintf(file,"triplet: %lld-by-%lld, nzmax: %lld nnz: %lld\n",  (long long int)m,  (long
     * long int)n, */
    /*         (long long int)nzmax,  (long long int)nz) ; */

    for (p = 0; p < nz; p++) {
      CHECK_IO(fscanf(file, "    %lld %lld : %32le\n", &val1, &val2, &val3), &info);
      DEBUG_PRINTF("    %lld %lld : %32le\n", val1, val2, val3);
      Ai[p] = (CS_INT)val1;
      Ap[p] = (CS_INT)val2;
      Ax[p] = val3;
      DEBUG_PRINTF("    %lld %lld : %g\n", (long long int)Ai[p], (long long int)Ap[p],
                   (double)Ax[p]);
    }
  }
  return out;
}

/* add an entry to triplet matrix only if value is not (nearly) null */
CS_INT CSparseMatrix_zentry(CSparseMatrix *T, CS_INT i, CS_INT j, double x, double threshold) {
  if (fabs(x) >= threshold) {
    return cs_entry(T, i, j, x);
  } else {
    return 1;
  }
}
/* add a block (col-major dense) to triplet matrix only if value is not (nearly) null */
CS_INT CSparseMatrix_block_dense_zentry(CSparseMatrix *T, CS_INT row_off, CS_INT col_off,
                                        double *x, CS_INT row_size, CS_INT col_size,
                                        double threshold) {
  for (CS_INT j = 0; j < col_size; ++j) {
    for (CS_INT i = 0; i < row_size; ++i) {
      // col-major

      CSparseMatrix_zentry(T, i + row_off, j + col_off, x[i + j * row_size], DBL_EPSILON);
    }
  }
  return 1;
}

/* add an entry to a symmetric triplet matrix only if value is not (nearly) null */
CS_INT CSparseMatrix_symmetric_zentry(CSparseMatrix *T, CS_INT i, CS_INT j, double x,
                                      double threshold) {
  if (fabs(x) >= threshold) {
    if (j <= i) {
      return cs_entry(T, i, j, x);
    }
  }
  return 1;
}

int CSparseMatrix_print(const CSparseMatrix *A, int brief) {
  cs_print(A, brief);
  return 0;
}

/* add an entry to triplet matrix */
CS_INT CSparseMatrix_entry(CSparseMatrix *T, CS_INT i, CS_INT j, double x) {
  return cs_entry(T, i, j, x);
}

/* add an entry to a symmetric triplet matrix */
CS_INT CSparseMatrix_symmetric_entry(CSparseMatrix *T, CS_INT i, CS_INT j, double x) {
  if (j <= i) {
    return cs_entry(T, i, j, x);
  }
  return 1;
}

int CSparseMatrix_print_in_file(const CSparseMatrix *A, int brief, FILE *file) {
  CS_INT m, n, nzmax, nz, p, j, *Ap, *Ai;
  double *Ax;
  if (!A) {
    fprintf(file, "(null)\n");
    return (0);
  }
  m = A->m;
  n = A->n;
  Ap = A->p;
  Ai = A->i;
  Ax = A->x;
  nzmax = A->nzmax;
  nz = A->nz;
  fprintf(file, "CSparse Version %d.%d.%d, %s.  %s\n", CS_VER, CS_SUBVER, CS_SUBSUB, CS_DATE,
          CS_COPYRIGHT);
  if (nz < 0) {
    fprintf(file, "%lld-by-%lld, nzmax: %lld nnz: %lld, 1-norm: %g\n", (long long int)m,
            (long long int)n, (long long int)nzmax, (long long int)Ap[n], cs_norm(A));
    for (j = 0; j < n; j++) {
      fprintf(file, "    col %lld : locations %lld to %lld\n", (long long int)j,
              (long long int)Ap[j], (long long int)Ap[j + 1] - 1);
      for (p = Ap[j]; p < Ap[j + 1]; p++) {
        fprintf(file, "      %lld : %g\n", (long long int)Ai[p], Ax ? Ax[p] : 1);
        if (brief && p > 20) {
          fprintf(file, "  ...\n");
          return (1);
        }
      }
    }
  } else {
    fprintf(file, "triplet: %lld-by-%lld, nzmax: %lld nnz: %lld\n", (long long int)m,
            (long long int)n, (long long int)nzmax, (long long int)nz);
    for (p = 0; p < nz; p++) {
      fprintf(file, "    %lld %lld : %g\n", (long long int)Ai[p], (long long int)Ap[p],
              Ax ? Ax[p] : 1);
      if (brief && p > 20) {
        fprintf(file, "  ...\n");
        return (1);
      }
    }
  }
  return (1);
}

int CSparseMatrix_print_in_Matlab_file(const CSparseMatrix *A, int brief, FILE *file) {
  CS_INT m, n, nzmax, nz, p, j, *Ap, *Ai;
  double *Ax;
  if (!A) {
    fprintf(file, "(null)\n");
    return (0);
  }
  m = A->m;
  n = A->n;
  Ap = A->p;
  Ai = A->i;
  Ax = A->x;
  nzmax = A->nzmax;
  nz = A->nz;
  /* fprintf(file,"CSparse Version %d.%d.%d, %s.  %s\n", CS_VER, CS_SUBVER, */
  /*         CS_SUBSUB, CS_DATE, CS_COPYRIGHT) ; */
  if (nz < 0) {
    fprintf(file, "%lld-by-%lld, nzmax: %lld nnz: %lld, 1-norm: %g\n", (long long int)m,
            (long long int)n, (long long int)nzmax, (long long int)Ap[n], cs_norm(A));
    for (j = 0; j < n; j++) {
      fprintf(file, "    col %lld : locations %lld to %lld\n", (long long int)j,
              (long long int)Ap[j], (long long int)Ap[j + 1] - 1);
      for (p = Ap[j]; p < Ap[j + 1]; p++) {
        fprintf(file, "      %lld : %10.50g\n", (long long int)Ai[p], Ax ? Ax[p] : 1);
        if (brief && p > 20) {
          fprintf(file, "  ...\n");
          return (1);
        }
      }
    }
  } else {
    for (p = 0; p < nz; p++) {
      fprintf(file, "    %lld %lld %20.16e\n", (long long int)Ai[p] + 1,
              (long long int)Ap[p] + 1, Ax ? Ax[p] : 1);
      if (brief && p > 20) {
        fprintf(file, "  ...\n");
        return (1);
      }
    }
    fprintf(file, "    %lld %lld %g\n", (long long int)m, (long long int)n, 0.0);
  }
  return (1);
}

CS_INT CSparseMatrix_to_dense(const CSparseMatrix *const A, double *B) {
  CS_INT p, j, m, n, nz, *Ap, *Ai;
  CS_ENTRY *Ax;

  if (!A) {
    printf("CSparseMatrix_to_dense :: A = null\n");
    return (1);
  }

  m = A->m;
  n = A->n;
  nz = A->nz;
  Ap = A->p;
  Ai = A->i;
  Ax = A->x;
  Ax = A->x;

  if (nz >= 0) {
    for (p = 0; p < nz; p++) {
      DEBUG_PRINTF("    %g %g : ", (double)(Ai[p]), (double)(Ap[p]));
      DEBUG_PRINTF("%g\n", Ax ? Ax[p] : 1);
      DEBUG_PRINTF("B %g\n", B[Ai[p] + Ap[p] * m]);
      B[Ai[p] + Ap[p] * m] = Ax[p];
    }
  } else {
    for (j = 0; j < n; j++) {
      DEBUG_PRINTF("    col %g : locations %g to %g\n", (double)j, (double)(Ap[j]),
                   (double)(Ap[j + 1] - 1));
      for (p = Ap[j]; p < Ap[j + 1]; p++) {
        DEBUG_PRINTF("      %g %g : ", (double)(Ai[p]), Ax ? Ax[p] : 1);
        B[Ai[p] + j * m] = Ax[p];
      }
    }
  }
  return (0);
}

CSparseMatrix *CSparseMatrix_alloc_for_copy(const CSparseMatrix *const m) {
  assert(m);
  CSparseMatrix *out = NULL;
  if (m->nz >= 0) /* triplet  */
  {
    out = cs_spalloc(m->m, m->n, m->nzmax, 1, 1);
  } else if (m->nz == -1) /* csc */
  {
    out = cs_spalloc(m->m, m->n, m->nzmax, 1, 0);
  } else if (m->nz == -2) /* csr  */
  {
    out = cs_spalloc(m->n, m->m, m->nzmax, 1, 0);
    out->nz = -2;
    out->m = m->m;
    out->n = m->n;
  } else {
    fprintf(stderr, "NM_copy :: error unknown type " CS_ID " for CSparse matrix\n", m->nz);
    exit(EXIT_FAILURE);
  }

  return out;
}

void CSparseMatrix_copy(const CSparseMatrix *const A, CSparseMatrix *B) {
  assert(A);
  assert(B);

  if (B->nzmax < A->nzmax) {
    B->x = (double *)realloc(B->x, (size_t)A->nzmax * sizeof(double));
    B->i = (CS_INT *)realloc(B->i, (size_t)A->nzmax * sizeof(CS_INT));
  } else if (!(B->x)) {
    B->x = (double *)malloc((size_t)A->nzmax * sizeof(double));
  }

  if (A->nz >= 0) {
    /* triplet */
    B->p = (CS_INT *)realloc(B->p, (size_t)A->nzmax * sizeof(CS_INT));
  } else if ((A->nz == -1) && (B->n < A->n)) {
    /* csc */
    B->p = (CS_INT *)realloc(B->p, ((size_t)A->n + 1) * sizeof(CS_INT));
  } else if ((A->nz == -2) && (B->m < A->m)) {
    /* csr */
    B->p = (CS_INT *)realloc(B->p, ((size_t)A->m + 1) * sizeof(CS_INT));
  }

  B->nzmax = A->nzmax;
  B->nz = A->nz;
  B->m = A->m;
  B->n = A->n;

  memcpy(B->x, A->x, (size_t)A->nzmax * sizeof(double));
  memcpy(B->i, A->i, (size_t)A->nzmax * sizeof(CS_INT));

  size_t size_cpy = 0;
  if (A->nz >= 0) {
    size_cpy = (size_t)A->nzmax;
  } else if (A->nz == -1) {
    size_cpy = (size_t)A->n + 1;
  } else if (A->nz == -2) {
    size_cpy = (size_t)A->m + 1;
  } else
    numerics_error("CSparseMatrix_copy", "Wrong nz value in CSparseMatrix.");

  memcpy(B->p, A->p, size_cpy * sizeof(CS_INT));
}

int CSparseMatrix_max_by_columns(const CSparseMatrix *A, double *max) {
  CS_INT p, j, n, *Ap;
  CS_ENTRY *Ax;
  double s;

  if (!CS_CSC(A) || !A->x) return (-1); /* check inputs */
  n = A->n;
  Ap = A->p;
  Ax = A->x;
  /* loop over the column */
  for (j = 0; j < n; j++) {
    /* loop over the element of the row */
    p = Ap[j];
    s = Ax[p];
    for (p = Ap[j]; p < Ap[j + 1]; p++) s = CS_MAX(Ax[p], s);
    max[j] = s;
  }
  return 0;
}
int CSparseMatrix_max_abs_by_columns(const CSparseMatrix *A, double *max) {
  CS_INT p, j, n, *Ap;
  CS_ENTRY *Ax;
  double s;

  if (!CS_CSC(A) || !A->x) return (-1); /* check inputs */
  n = A->n;
  Ap = A->p;
  Ax = A->x;
  /* loop over the column */
  for (j = 0; j < n; j++) {
    /* loop over the element of the row */
    p = Ap[j];
    s = fabs(Ax[p]);
    for (p = Ap[j]; p < Ap[j + 1]; p++) s = CS_MAX(fabs(Ax[p]), s);
    max[j] = s;
  }
  return 0;
}
